/*
 *	
 * main.c
 *
 * Demos:
 * 1) Run YARP server 
 *        > yarp server
 * 2) Run tracker device
 *        > tracker.exe
 * 3) read from tracker device 
 *        > yarp read ... /tracker
 *
 * Options
 *    --period 0.002
 *    --type polhemus|circle
 *    --mode streaming|polling
 *    --c1 x --c2 y --c3 z --c4 azimuth --c5 elevation -c6 roll 
*/

#include <stdio.h>

#include "TrackerDevice.h"

#include <yarp/os/all.h>
#include <yarp/sig/all.h>
#include <yarp/dev/all.h>

using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::dev;

int main(int argcp, char **argv)
{
    Network yarp;

	DriverCreator *tracker_factory =
		new DriverCreatorOf<TrackerDevice>("tracker","inertial","TrackerDevice");
	Drivers::factory().add(tracker_factory); // hand factory over to YARP

    //Property options;
    PolyDriver driver;

	Property options;
	options.fromCommand(argcp,argv);
    //options.fromConfigFile("myfile.ini");
    //...
	options.put("device","inertial");
    options.put("subdevice","tracker");
    options.put("name","/tracker");
	options.put("period",0.002); // run intertial sevfer at 2 ms (to handle 240 Hz) 

    if (!driver.open(options)) {
		printf("Cannot open driver\n");
        return 1;
    } else {
        printf("Driver opened\n");
    }

    IGenericSensor *sensor = NULL;
    driver.view(sensor);
    if (!sensor) {
 		printf("Cannot view sensor interface\n");
        driver.close();
        return 1;
    } else {
        printf("Sensor interface exposed\n");
    }

	// test
//	Vector v;
//	sensor->read(v);
//	printf("test: %s\n", v.toString().c_str());

    Time::delay(5000);
   
    return 0;

}
